//
// Created by yongj on 2021/7/30.
//

#include "P36.h"

//uint16_t motor_1_angle;
//uint16_t motor_2_angle;
//int16_t motor_1_speed;
//int16_t motor_2_speed;
//int16_t motor_1_torque;
//int16_t motor_2_torque;
//int16_t motor_1_current;
//int16_t motor_2_current;

car_t Balance_car;

repeating_timer_t timer_getSpeed;
repeating_timer_t timer_sendDATA;
repeating_timer_t timer_keep_control;
//repeating_timer_t timer_PID_MATH;

void p36_init() {
    MCP2515_Pin_init();
    mcp2515_init(CAN_1000KBPS, MCP_8MHz);

    init_Mask(0, 0, 0xFFF);
    init_Mask(1, 0, 0xFFF);
    init_Filt(0, 0, 0x201);
    init_Filt(1, 0, 0x201);
    init_Filt(2, 0, 0x202);
    init_Filt(3, 0, 0x202);
    init_Filt(4, 0, 0x202);
    init_Filt(5, 0, 0x202);

    //初始化pid参数
    PID_init(&Balance_car.Left.motor_pid);
    PID_init(&Balance_car.Right.motor_pid);

    //初始化测速时钟中断
    while (!add_repeating_timer_ms(-2, getSpeed, NULL, &timer_getSpeed));

    //初始化电机转速维持
//    while (!add_repeating_timer_ms(1900, keep_control, NULL, &timer_keep_control));

    //初始化测速反馈时钟中断
//    while (!add_repeating_timer_ms(-50, sendDATA, NULL, &timer_sendDATA));
}

void p36_get_motor_state() {
    uint8_t data_buf[6];
    uint8_t ext, rtrBit, len;
    unsigned long get_id;
    mcp2515_read_canMsg(MCP_READ_RX0, &get_id, &ext, &rtrBit, &len, data_buf);
    if (get_id == MOTOR_1_ID) {
        Balance_car.Left.angle = (data_buf[MOTOR_ANGLE_H] << 8) + data_buf[MOTOR_ANGLE_L];
        Balance_car.Left.motor_pid.ActualRPM = Balance_car.Left.speed =
                (data_buf[MOTOR_RPM_H] << 8) + data_buf[MOTOR_RPM_L];
        Balance_car.Left.torque = (data_buf[MOTOR_TORQUE_H] << 8) + data_buf[MOTOR_TORQUE_L];
    } else if (get_id == MOTOR_2_ID) {
        Balance_car.Right.angle = (data_buf[MOTOR_ANGLE_H] << 8) + data_buf[MOTOR_ANGLE_L];
        Balance_car.Right.motor_pid.ActualRPM = Balance_car.Right.speed =
                (data_buf[MOTOR_RPM_H] << 8) + data_buf[MOTOR_RPM_L];
        Balance_car.Right.torque = (data_buf[MOTOR_TORQUE_H] << 8) + data_buf[MOTOR_TORQUE_L];
    }
}

float p36_get_motor_angle(unsigned long id) {
    float angle_data;
    if (id == MOTOR_1_ID) {
        angle_data = Balance_car.Left.angle;
    } else if (id == MOTOR_2_ID) {
        angle_data = Balance_car.Right.angle;
    }
    return angle_data * 0.04395;
}

int16_t p36_get_motor_speed(unsigned long id) {
    if (id == MOTOR_1_ID)
        return Balance_car.Left.speed;
    else if (id == MOTOR_2_ID) {
        return -Balance_car.Right.speed;
    }
}

uint16_t p36_get_motor_torque(unsigned long id) {
    if (id == MOTOR_1_ID)
        return Balance_car.Left.torque;
    else if (id == MOTOR_2_ID) {
        return -Balance_car.Right.torque;
    }
}

void p36_put_motor_current() {
    uint8_t txdataH[8] = {Balance_car.Left.motor_pid.CurrentOUT >> 8, Balance_car.Left.motor_pid.CurrentOUT,
                          Balance_car.Right.motor_pid.CurrentOUT >> 8, Balance_car.Right.motor_pid.CurrentOUT, 0x00,
                          0x00,
                          0x00, 0x00};
    mcp2515_write_canMsg(MCP_TXB0SIDH, 0x200, 0, 0, 8, txdataH);
}

void p36_set_motor_current(unsigned long id, int16_t current) {
    if (id == MOTOR_1_ID)
        Balance_car.Left.motor_pid.CurrentOUT = current;
    if (id == MOTOR_2_ID)
        Balance_car.Right.motor_pid.CurrentOUT = -current;
}

bool getSpeed() {
    p36_get_motor_state();
//    if (Balance_car.Left.motor_pid.setRPM) {
    PID_get(&Balance_car.Left.motor_pid);
//    } else {
//        Balance_car.Left.motor_pid.integral = 0;
//    }
//    if (Balance_car.Right.motor_pid.setRPM) {
    PID_get(&Balance_car.Right.motor_pid);
//    } else {
//        Balance_car.Right.motor_pid.integral = 0;
//    }
    p36_put_motor_current();
    return true;
}

bool sendDATA() {
//    printf("angle%f, speed%d, torque%02x\n", p36_get_motor_angle(MOTOR_1_ID), p36_get_motor_speed(MOTOR_1_ID),
//           p36_get_motor_torque(MOTOR_1_ID));
//    printf("angle%f, speed%d, torque%02x\n", p36_get_motor_angle(MOTOR_2_ID), p36_get_motor_speed(MOTOR_2_ID),
//           p36_get_motor_torque(MOTOR_2_ID));
//    printf("Kp%f Ki%f Kd%f\n", Balance_car.Right.motor_pid.Kp, Balance_car.Right.motor_pid.Ki,
//           Balance_car.Right.motor_pid.Kd);
    printf("d1:%d,%d,%d,%d\n", Balance_car.Left.motor_pid.ActualRPM,
           Balance_car.Left.motor_pid.setRPM, Balance_car.Right.motor_pid.ActualRPM,
           Balance_car.Right.motor_pid.setRPM);
    return true;
}

bool keep_control() {
    p36_put_motor_current();
    return true;
}

void PID_init(pid *setPID) {
    setPID->Kp = 1.54;
    setPID->Ki = 0.017;
    setPID->Kd = 6.8;
    setPID->setRPM = 0;
    setPID->ActualRPM = 0;
    setPID->err = 0;
    setPID->err_last = 0;
    setPID->err_lim = 20;
    setPID->integral = 0;
    setPID->integral_lim = 50000;
    setPID->CurrentOUT = 0;
}

void PID_get(pid *pid_set) {
    pid_set->err = pid_set->setRPM - pid_set->ActualRPM;

    pid_set->integral += pid_set->err;
    if (pid_set->integral > pid_set->integral_lim) {
        pid_set->integral = pid_set->integral_lim;
    } else if (pid_set->integral < -pid_set->integral_lim) {
        pid_set->integral = -pid_set->integral_lim;
    } else if (pid_set->setRPM == 0) {
        pid_set->integral = 0;
    }

    pid_set->CurrentOUT = pid_set->Kp * pid_set->err + pid_set->Ki * pid_set->integral +
                          pid_set->Kd * (pid_set->err - pid_set->err_last);

    if (pid_set->CurrentOUT > MAXCurrent)
        pid_set->CurrentOUT = MAXCurrent;
    else if (pid_set->CurrentOUT < -MAXCurrent)
        pid_set->CurrentOUT = -MAXCurrent;

    pid_set->err_last = pid_set->err;
}

void PID_set(float Kp, float Ki, float Kd) {
    Balance_car.Left.motor_pid.Kp = Kp;
    Balance_car.Left.motor_pid.Ki = Ki;
    Balance_car.Left.motor_pid.Kd = Kd;
    Balance_car.Right.motor_pid.Kp = Kp;
    Balance_car.Right.motor_pid.Ki = Ki;
    Balance_car.Right.motor_pid.Kd = Kd;
}

void PID_set_Kp(float Kp) {
    Balance_car.Left.motor_pid.Kp = Kp;
    Balance_car.Right.motor_pid.Kp = Kp;
}

void PID_set_Ki(float Ki) {
    Balance_car.Left.motor_pid.Ki = Ki;
    Balance_car.Right.motor_pid.Ki = Ki;
}

void PID_set_Kd(float Kd) {
    Balance_car.Left.motor_pid.Kd = Kd;
    Balance_car.Right.motor_pid.Kd = Kd;
}

void p36_set_motor_RPM(unsigned long id, int16_t RPM) {
    if (id == MOTOR_1_ID)
        Balance_car.Left.motor_pid.setRPM = RPM;
    if (id == MOTOR_2_ID)
        Balance_car.Right.motor_pid.setRPM = -RPM;
}

int p36_get_motor_RPM(unsigned long id) {
    if (id == MOTOR_1_ID)
        return Balance_car.Left.motor_pid.ActualRPM;
    if (id == MOTOR_2_ID)
        return Balance_car.Right.motor_pid.ActualRPM;
}

void p36_state_set_STOP() {
    p36_set_motor_RPM(MOTOR_1_ID, 0);
    p36_set_motor_RPM(MOTOR_2_ID, 0);
}

int16_t balance(float Angle,float Gyro) {
    float kp = 530*0.8, kd = 1.3*0.8;
    int balance;
    if (abs(Angle) < 60) {
        balance = kp * (Angle + 1.5444) + kd * Gyro;   //===计算平衡控制的电机PWM  PD控制   kp是P系数 kd是D系数
        if (balance > 21000) {
            balance = 21000;
        } else if (balance < -21000) {
            balance = -21000;
        }
    } else {
        return 0;

    }
    return balance;
}

int velocity(int Encoder_Left,int Encoder_Right,float Angle){
    float velocity_KP = 1.8,velocity_KI;
    static float Velocity,Encoder_Least,Encoder,Movement;
    static float Encoder_Integral;
    Encoder_Least =(Encoder_Left+Encoder_Right)-0;                    //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零）
    Encoder *= 0.8;		                                                //===一阶低通滤波器
    Encoder += Encoder_Least*0.2;	                                    //===一阶低通滤波器
    Encoder_Integral +=Encoder;                                       //===积分出位移 积分时间：10ms
    Encoder_Integral=Encoder_Integral-Movement;                       //===接收遥控器数据，控制前进后退
    if(Encoder_Integral>10000)  	Encoder_Integral=10000;             //===积分限幅
    if(Encoder_Integral<-10000)		Encoder_Integral=-10000;            //===积分限幅
    Velocity=Encoder*velocity_KP+Encoder_Integral*velocity_KI;        //===速度控制
    if(Angle<-40||Angle>40) 			Encoder_Integral=0;     						//===电机关闭后清除积分
    return Velocity;
}